#include "cmsis_os.h"
#include "pid.h"
#include "remote_control.h"
#include "NUC_communicate.h"
#include "robot_total_mode_task.h"
#include "CAN_receive.h"
#include "main.h"
#include "detect_task.h"
#include "OLED.h"
#include "referee.h"
#include "robot_config.h"
#include "struct_typedef.h"
#include "debug_config.h"
#include "shoot_task.h"
#include "user_lib.h"
#ifndef SENTRY_CHASSIS

// 注意：当遥控器右拨杆指向中央的时候，才允许鼠标操控

//*************任务设置
#define SHOOT_TASK_INIT_TIME 500
#define SHOOT_CTRL_TIME 5
#ifdef INFANTRY
#define M2006_MOTOR_SPEED_PID_KP 4.0f
#define M2006_MOTOR_SPEED_PID_KI 0.5f
#define M2006_MOTOR_SPEED_PID_KD 0.0f
#define M3508_MOTOR_SPEED_PID_KP 3.0f
#define M3508_MOTOR_SPEED_PID_KI 0.2f
#define M3508_MOTOR_SPEED_PID_KD -1.0f
#define FRIC_FILTER 1.0f
#endif
#ifdef HERO
#define M2006_MOTOR_SPEED_PID_KP 3.0f
#define M2006_MOTOR_SPEED_PID_KI 0.0f
#define M2006_MOTOR_SPEED_PID_KD 0.0f
#define M3508_MOTOR_SPEED_PID_KP 5.0f
#define M3508_MOTOR_SPEED_PID_KI 0.0f
#define M3508_MOTOR_SPEED_PID_KD 0.0f
#define FRIC_FILTER 1.0f
#endif
#ifdef SENTRY
#define M2006_MOTOR_SPEED_PID_KP 4.0f
#define M2006_MOTOR_SPEED_PID_KI 0.5f
#define M2006_MOTOR_SPEED_PID_KD 0.0f
#define M3508_MOTOR_SPEED_PID_KP 3.0f
#define M3508_MOTOR_SPEED_PID_KI 0.2f
#define M3508_MOTOR_SPEED_PID_KD -1.0f
#define FRIC_FILTER 1.0f
#endif

#define MAX_MOTOR_CAN_CURRENT 16000.0f
#define M3508_MOTOR_SPEED_PID_MAX_OUT MAX_MOTOR_CAN_CURRENT
#define M3508_MOTOR_SPEED_PID_MAX_IOUT 2000.0f

#define MAX_MOTOR_CAN_CURRENT_M2006 10000.0f
#define M2006_MOTOR_SPEED_PID_MAX_OUT MAX_MOTOR_CAN_CURRENT_M2006
#define M2006_MOTOR_SPEED_PID_MAX_IOUT 2000.0f

//**********************卡弹相关***********************//
#ifdef INFANTRY
#define STUCK_TIME_LIMIT 800   // 当在发射一发的模式中持续一段时间不退出后，进入卡弹解决模式  //500
#define SOLVING_TIME_LIMIT 200 // 在尝试卡弹解决模式中停留的时间。 //300

#endif

#ifdef HERO
#define STUCK_TIME_LIMIT 1200  // 当在发射一发的模式中持续一段时间不退出后，进入卡弹解决模式  //500
#define SOLVING_TIME_LIMIT 200 // 在尝试卡弹解决模式中停留的时间。 //300

#endif

#ifdef SENTRY
#define STUCK_TIME_LIMIT 800   // 当在发射一发的模式中持续一段时间不退出后，进入卡弹解决模式  //500
#define SOLVING_TIME_LIMIT 200 // 在尝试卡弹解决模式中停留的时间。 //300

#endif

/********************单发圈数控制相关*******************/
#define TRIGGER_RANGE 0.25f
#ifdef INFANTRY
#define TRIGGER_ROUNDS 3.6f
#endif
#ifdef HERO
#define TRIGGER_ROUNDS 6.045f // 6.045圈一发

#endif
#ifdef SENTRY
#define TRIGGER_ROUNDS 4.5f
#endif

#define WITHOUT_HEAT_DETECT 2    // 前几发不做已发射判断控制
#define TRIGGER_STOP_TO_MULTI 10 // 拨弹轮低于这个速度才会继续发射

const static fp32 motor_speed_pid[3] = {M3508_MOTOR_SPEED_PID_KP, M3508_MOTOR_SPEED_PID_KI, M3508_MOTOR_SPEED_PID_KD};
const static fp32 trigger_motor_speed_pid[3] = {M2006_MOTOR_SPEED_PID_KP, M2006_MOTOR_SPEED_PID_KI, M2006_MOTOR_SPEED_PID_KD};

enum TriggerMode_e
{
    TRIGGERMODE_E_STOP,       // 停止发射
    TRIGGERMODE_E_SHOOTONE,   // 发射一个，靠拨弹轮减速前的电机转4圈。对应弹盘轮子转一格
    TRIGGERMODE_E_SHOOTMULTI, // 多发，其实是等待进入发射一个的状态
    TRIGGERMODE_E_STUCKSOLVE, // 尝试解决卡弹
};

struct TriggerControl_t
{
    const uint16_t *ECDPoint; // 电机ECD所在位置
    uint16_t initECD;         // 初始ECD
    uint16_t nowECD;          // 现在的ECD
    uint16_t lastEcd;
    int16_t stepEcd;
    fp32 nowRounds;  // 现在转过的圈数
    fp32 lastRounds; // 上一次转过的圈数
    fp32 stepRounds; // 这发转过的圈数
};

//**********缓冲指令
struct InstructBuff_t
{
    uint8_t turnOnFric; // 为1时表示指令开启摩擦轮，为0时表示指令关闭摩擦轮
    uint8_t shootOne;
    uint8_t shootMulti;
    uint8_t withoutHeatDetect;
};

//*******************全局变量********//
// 摩擦轮开启
uint8_t fricOn = 0;

// 拨弹轮开启
uint8_t triggerOn = 0;
uint8_t reverse = 0;

static struct InstructBuff_t insBuff; // 全局发射机构控制指令缓冲
// 在云台任务中控制速度
static int zeroCurrentMark; // 当离线时，直接发送零电流的时候
const static RC_ctrl_t *rc_p;
static const fromNUC_t *nuc_p;                  // NUC数据位置。未来制造全自动机器人时需要
static const remote_control_t *remoteControl_p; // 图传链路控制
static int16_t giveShootCurrent[2];
static int16_t giveTriggerCurrent;
static pid_type_def shootMotorPIDs[2], triggerMotorPID;
static fp32 wantedVShootMotor[2], wantedVTriggerMotor;
static fp32 presentVShootMotor[2], presentVTriggerMotor;
static first_order_filter_type_t fricFilter[2];
static struct TriggerControl_t triggerCtrl;
static enum TriggerMode_e triggerMode;
static uint8_t *robotMode;
static uint16_t heatlimit;
static uint16_t heat;
#ifdef GIMBAL_WITHOUT_IMU
static const int16_t *fricUpCurrent_p;
static const int16_t *triggerUpCurrent_p;
#endif
static uint8_t lastHasBeenShoot = 1; // 弹丸不是瞬发,1代表前面一发弹丸已发射状态；
static uint16_t lastHeat = 0;
static uint8_t numsOfBeenShoot = 0;

static void init_shoot_PIDs(void)
{
    uint8_t i;
    for (i = 0; i < 2; i++)
        PID_init(&shootMotorPIDs[i], PID_POSITION, motor_speed_pid, M3508_MOTOR_SPEED_PID_MAX_OUT, M3508_MOTOR_SPEED_PID_MAX_IOUT);
    PID_init(&triggerMotorPID, PID_POSITION, trigger_motor_speed_pid, M2006_MOTOR_SPEED_PID_MAX_OUT, M2006_MOTOR_SPEED_PID_MAX_IOUT);
}

static void init_shoot_modes(void)
{
    // 初始化指令
    insBuff.shootMulti = 0;
    insBuff.shootOne = 0;
    insBuff.turnOnFric = 0;
    triggerMode = TRIGGERMODE_E_STOP;
    fricOn = 0;
    triggerOn = 0;
}

static void init_shoot_filter(void)
{
    const static fp32 num[1] = {FRIC_FILTER};
    first_order_filter_init(fricFilter, SHOOT_CTRL_TIME, num);
    first_order_filter_init(fricFilter + 1, SHOOT_CTRL_TIME, num);
}

static void check_NUC_offLine_and_set_insBuf(void)
{
    if (*robotMode == ROBOT_STATE_AUTO)
    {
        if (toe_is_error(MINI_PC_TOE))
        {
            insBuff.turnOnFric = 0;
            insBuff.shootMulti = 0;
            insBuff.shootOne = 0;
            zeroCurrentMark = 1;
        }
    }
    else if (*robotMode == ROBOT_STATE_POWERLESS)
    {
        zeroCurrentMark = 1;
    }
}

static void init_trigger_ECD_rounds_monitor() // 初始化拨弹轮圈数监控
{
#ifdef SENTRY
    triggerCtrl.ECDPoint = &(get_motor_measure_point(SHOOT_DOWN_TRIGGER)->ecd);
#else
    triggerCtrl.ECDPoint = &(get_motor_measure_point(SHOOT_TRIGGER)->ecd);
#endif
    triggerCtrl.initECD = *(triggerCtrl.ECDPoint);
    triggerCtrl.nowRounds = 0;
    triggerCtrl.lastEcd = *(triggerCtrl.ECDPoint);
    triggerCtrl.stepEcd = 0;
    triggerCtrl.lastRounds = 0;
    triggerCtrl.stepRounds = 0;
}

#ifdef STM_STUDIO_SHOOT_CONTROL
static uint32_t mousePressTime = 0;
#endif

static void get_instruction_and_buff(void)
{
#ifndef STM_STUDIO_SHOOT_CONTROL
    static uint32_t mousePressTime = 0;
#endif
    static uint8_t up = 0, down = 0;
    zeroCurrentMark = 0;

    if (*robotMode == ROBOT_STATE_AUTO)
    {
        if (game_not_started())
        {
            insBuff.turnOnFric = 0;
        }
        else
        {
            insBuff.turnOnFric = 1;
        }
        if (shoot_NUC_is_available())
        {
#ifdef LED_AUTO_SHOOT
            aRGB_led_show(0xFFFF0000);
#endif
            if (nuc_p->shoot == 1)
            {
#ifdef LED_AUTO_SHOOT
                aRGB_led_show(0xFF00FF00);
#endif
                insBuff.turnOnFric = 1;
                insBuff.shootOne = 1;
            }
            shoot_clear_read_mark();
        }
    }
    else
    {
        static uint16_t shiftPressTime = 0;
        // 通过按键开启摩擦轮（也许用处并不大，有了遥控器的就足够了，但还是用来确保开启了摩擦轮吧）
        if (remoteControl_p->keyboard_value & KEY_FRIC_ON)
        {
            if (shiftPressTime < PRESS_LONG_TIME)
                shiftPressTime += SHOOT_CTRL_TIME;
            else // 已经按了足够长的时间了，可以不用再增加了，
            {
                insBuff.turnOnFric = 1; // 想要开启摩擦轮
                lastHasBeenShoot = 1;
                insBuff.withoutHeatDetect = 1;
            }
        }
        else
        {
            if (shiftPressTime > 0)
            {
                insBuff.turnOnFric = 1; // 想要开启摩擦轮
                lastHasBeenShoot = 1;
            }
        }
        if (!robot_is_auxiliaryTarget())
        {
            if (key_has_changed())
            {
#ifdef HERO
                if (rc_p->mouse.press_l) // 按下左键
                {
                    if (mousePressTime < PRESS_LONG_TIME)
                        mousePressTime += SHOOT_CTRL_TIME;
                    else // 已经按了足够长的时间了，可以不用再增加了，
                    {
                        insBuff.shootMulti = 1;
                    }
                }
                else // 左键抬起
                {
                    if (mousePressTime < PRESS_LONG_TIME) // 拨下并很快抬起，发射一颗
                        if (mousePressTime > 0)
                        {
                            insBuff.shootOne = 1;
                        }
                    mousePressTime = 0;
                    insBuff.shootMulti = 0;
                }
#endif
#ifdef INFANTRY
                if (remoteControl_p->left_button_down) // 按下左键
                {
                    if (mousePressTime < KEEP_SHOOT_TIME)
                    {
                        if (lastHasBeenShoot)
                        {
                            mousePressTime += SHOOT_CTRL_TIME;
                            insBuff.shootOne = 1;
                        }
                        insBuff.shootMulti = 0;
                    }
                    else // 已经按了足够长的时间了，可以不用再增加了，
                    {
                        insBuff.shootOne = 0;
                        insBuff.shootMulti = 0;
                    }
                }
                else // 左键抬起
                {
                    mousePressTime = 0;
                    insBuff.shootOne = 0;
                    insBuff.shootMulti = 0;
                }
#endif
            }
            else
            {
#ifdef SENTRY
                if (switch_is_down(rc_p->rc.ch[RC_CHOOSE_GIMBAL]))
                {
#endif
                    if (switch_is_up(rc_p->rc.ch[RC_CONTROL_SHOOT]))
                    {
                        up = 1;
                    }
                    else if (switch_is_mid(rc_p->rc.ch[RC_CONTROL_SHOOT]))
                    {
                        // 通过上拨和下拨，实现摩擦轮状态的控制
                        if (up == 1) // 从上到中 下拨
                        {
                            if (fricOn)
                            {
                                insBuff.shootMulti = 0;
                                insBuff.shootOne = 0;
                                insBuff.turnOnFric = 0; // 希望关闭摩擦轮
                            }
                            else
                            {
                                insBuff.turnOnFric = 1; // 希望开启摩擦轮
                            }
                            up = 0;
                        }
                        if (down == 1) // 刚刚是下方，从下到中的上拨
                        {
                            insBuff.shootMulti = 0;               // 停止连发
                            if (mousePressTime < PRESS_LONG_TIME) // 拨下并很快抬起，发射一颗
                                if (mousePressTime > 0)
                                {
                                    insBuff.shootOne = 1;
                                }
                            mousePressTime = 0;
                            down = 0;
                        }
                    }
                    else if (switch_is_down(rc_p->rc.ch[RC_CONTROL_SHOOT]))
                    {
                        down = 1;
                        if (mousePressTime < PRESS_LONG_TIME)
                            mousePressTime += SHOOT_CTRL_TIME;
                        else
                        {
                            insBuff.shootMulti = 1;
                        }
                    }
#ifdef SENTRY
                }
#endif
            }
        }
        else
        {
            if (game_not_started())
            {
                insBuff.turnOnFric = 0;
            }
            else
            {
                insBuff.turnOnFric = 1;
            }
            if (shoot_NUC_is_available())
            {
                if (nuc_p->shoot == 1)
                {
                    insBuff.turnOnFric = 1;
                    insBuff.shootOne = 1;
                }
                shoot_clear_read_mark();
            }
        }
    }
    if (toe_is_error(DBUS_TOE))
    {
        zeroCurrentMark = 1;
        triggerOn = 0;
        fricOn = 0;
        triggerMode = TRIGGERMODE_E_STOP;
        insBuff.turnOnFric = 0;
        insBuff.shootOne = 0;
        insBuff.shootMulti = 0;
    }
}

void get_last_has_been_shoot(void)
{
#ifdef INFANTRY
    get_shoot_heat_limit_and_heat_infantry(&heatlimit, &heat);
    if (!lastHasBeenShoot && lastHeat < heat)
    {
        lastHasBeenShoot = 1;
        if (numsOfBeenShoot <= WITHOUT_HEAT_DETECT)
        {
            numsOfBeenShoot++;
        }
    }
#endif
#ifdef HERO
    get_shoot_heat_limit_and_heat_hero(&heatlimit, &heat);
    if (!lastHasBeenShoot && lastHeat < heat)
    {
        lastHasBeenShoot = 1;
        if (numsOfBeenShoot <= WITHOUT_HEAT_DETECT)
        {
            numsOfBeenShoot++;
        }
    }
#endif
#ifdef SENTRY
    heatlimit=get_shoot_heat_limit();
    heat=get_shoot_heat_1();
    if (!lastHasBeenShoot && lastHeat < heat)
    {
        lastHasBeenShoot = 1;
        if (numsOfBeenShoot <= WITHOUT_HEAT_DETECT)
        {
            numsOfBeenShoot++;
        }
    }
#endif
    lastHeat = heat;
}

static void heat_detect(void)
{
    if (insBuff.withoutHeatDetect)
    {
        insBuff.withoutHeatDetect = 0;
        return;
    }
#ifdef INFANTRY
    if (!game_not_started())
    {
        if (heat >= (heatlimit - 30) || (numsOfBeenShoot > WITHOUT_HEAT_DETECT && !lastHasBeenShoot))
        {
            insBuff.shootMulti = 0;
            insBuff.shootOne = 0;
        }
    }
    else
    {
        if (heat >= (heatlimit - 30))
        {
            insBuff.shootMulti = 0;
            insBuff.shootOne = 0;
        }
    }
#endif
#ifdef HERO
    if (!game_not_started())
    {
        if (heat >= (heatlimit - 130) || (numsOfBeenShoot > WITHOUT_HEAT_DETECT && !lastHasBeenShoot))
        {
#ifdef LED_SHOOT_HEAT_DETECT
            aRGB_led_show(0xFFFF0000);
#endif
            insBuff.shootMulti = 0;
            insBuff.shootOne = 0;
        }
    }
    else
    {
#ifdef LED_SHOOT_HEAT_DETECT
        aRGB_led_show(0xFF00FF00);
#endif
        if (heat >= (heatlimit - 130))
        {
            insBuff.shootMulti = 0;
            insBuff.shootOne = 0;
        }
    }
#endif
#ifdef SENTRY
    if (!game_not_started())
    {
        if (heat >= (heatlimit - 40) || (numsOfBeenShoot > WITHOUT_HEAT_DETECT && !lastHasBeenShoot))
        {
            insBuff.shootMulti = 0;
            insBuff.shootOne = 0;
        }
    }
    else
    {
        if (heat >= (heatlimit - 40))
        {
            insBuff.shootMulti = 0;
            insBuff.shootOne = 0;
        }
    }
#endif
    if (insBuff.shootOne || insBuff.shootMulti)
    {
        lastHasBeenShoot = 0;
    }
}

static void fric_mode_change(void)
{
    // 等待拨弹轮关闭时再关闭摩擦轮
    if ((!(insBuff.turnOnFric)) || (*robotMode) == ROBOT_STATE_POWERLESS)
    {
        if (triggerMode == TRIGGERMODE_E_STOP)
        {
            fricOn = 0;
            insBuff.shootMulti = 0;
            insBuff.shootOne = 0;
            insBuff.turnOnFric = 0;
        }
    }
    else
    {
        fricOn = 1;
        // insBuff.turnOnFric = 0;
    }
}

static fp32 trigger_rounds_format(fp32 in)
{
    if (in < 0)
    {
        in *= -1;
    }
    in -= (int)(in / TRIGGER_ROUNDS) * TRIGGER_ROUNDS;
    return in;
}

static void trigger_mode_change(void)
{
    static uint32_t lastShootMultiTime = 0;
    static uint32_t shootOneEnterTime = 0;
    static uint32_t solveStuckEnterTime = 0; // 在发射一发的状态中停留的时间
    static uint32_t nowTime = 0;
    nowTime = HAL_GetTick();
    if (!insBuff.turnOnFric) // 摩擦轮关闭状态，始终让拨弹轮关闭
    {
        triggerMode = TRIGGERMODE_E_STOP;
        return;
    }
    // 对每个状态枚举
    if (triggerMode == TRIGGERMODE_E_STOP)
    {
        // 机器人不处于无力状态时，接收从stop状态转变为其他状态的指令
        if ((*robotMode) != ROBOT_STATE_POWERLESS)
        {
            if (insBuff.shootOne)
            {
                triggerMode = TRIGGERMODE_E_SHOOTONE;
                shootOneEnterTime = nowTime;
                insBuff.shootOne = 0; // 清除缓冲的指令
            }
            else if (insBuff.shootMulti
            )
            {
                triggerMode = TRIGGERMODE_E_SHOOTMULTI;
                insBuff.shootMulti = 0; // 清除缓冲的指令
            }
        }
    }
    else if (triggerMode == TRIGGERMODE_E_SHOOTONE)
    {
        triggerCtrl.stepRounds = triggerCtrl.nowRounds - triggerCtrl.lastRounds;
        if (triggerCtrl.stepRounds > TRIGGER_ROUNDS || triggerCtrl.stepRounds < -TRIGGER_ROUNDS || (trigger_rounds_format(triggerCtrl.nowRounds) <= (TRIGGER_ROUNDS + TRIGGER_RANGE) && trigger_rounds_format(triggerCtrl.nowRounds) >= (TRIGGER_ROUNDS - TRIGGER_RANGE)))
        {
            triggerMode = TRIGGERMODE_E_STOP;
            triggerCtrl.lastRounds = triggerCtrl.nowRounds;
        }
        else if (nowTime - shootOneEnterTime > STUCK_TIME_LIMIT)
        {
            triggerMode = TRIGGERMODE_E_STUCKSOLVE; // 到达了卡弹的临界
            solveStuckEnterTime = nowTime;
        }
        // 否则留在这个状态
    }
    else if (triggerMode == TRIGGERMODE_E_SHOOTMULTI)
    {
        if ((nowTime - lastShootMultiTime) >= SHOOT_MULTI_TIME_GAP)
        {
            lastShootMultiTime = nowTime;
            triggerMode = TRIGGERMODE_E_SHOOTONE;
            shootOneEnterTime = nowTime;
        }
    }
    else if (triggerMode == TRIGGERMODE_E_STUCKSOLVE)
    {
        if ((nowTime - solveStuckEnterTime) >= SOLVING_TIME_LIMIT)
        {
            triggerMode = TRIGGERMODE_E_STOP;
        }
    }
}

static void detect_fric_offLine(void)
{
#ifdef SENTRY
    if ((toe_is_error(SHOOT_DOWN_FRIC_L_TOE)) || (toe_is_error(SHOOT_DOWN_FRIC_R_TOE)))
    {
        fricOn = 0;
        triggerOn = 0;
        insBuff.shootMulti = 0;
        insBuff.shootOne = 0;
        insBuff.turnOnFric = 0;
    }
#else
    if ((toe_is_error(SHOOT_FRIC_L_TOE)) || (toe_is_error(SHOOT_FRIC_R_TOE)))
    {
        fricOn = 0;
        triggerOn = 0;
        insBuff.shootMulti = 0;
        insBuff.shootOne = 0;
        insBuff.turnOnFric = 0;
    }
#endif
}

static void set_speed_by_mode(void)
{
    if (fricOn)
    {
        wantedVShootMotor[0] = -SHOOT_SPEED_LIMIT; // 实验测试转动方向。反过来可以实验卡弹时的退弹
        wantedVShootMotor[1] = SHOOT_SPEED_LIMIT;
    }
    else
    {
        wantedVShootMotor[0] = 0;
        wantedVShootMotor[1] = 0;
    }
    if (triggerMode == TRIGGERMODE_E_SHOOTONE || triggerMode == TRIGGERMODE_E_STUCKSOLVE)
        triggerOn = 1;
    else
        triggerOn = 0;

    reverse = 0;
    if (triggerMode == TRIGGERMODE_E_STUCKSOLVE)
    {
        reverse = 1;
    }

    if (triggerOn)
    {
#ifdef INFANTRY
        wantedVTriggerMotor = reverse ? (0) : (-SHOOT_TRIGGER_SPEED_LIMIT); // 若正在解决stuck(reverse)，则以逆向v/4的速度转动
#endif
#ifdef HERO
        wantedVTriggerMotor = reverse ? (0) : (SHOOT_TRIGGER_SPEED_LIMIT);
#endif
#ifdef SENTRY
        wantedVTriggerMotor = reverse ? (0) : (-SHOOT_TRIGGER_SPEED_LIMIT);
#endif
    }
    else
    {
        wantedVTriggerMotor = 0;
    }
}

static void calc_give_current(void)
{
    int i;
    for (i = 0; i < 2; i++)
        PID_calc(&shootMotorPIDs[i], presentVShootMotor[i], wantedVShootMotor[i]);
    for (i = 0; i < 2; i++)
        giveShootCurrent[i] = shootMotorPIDs[i].out;
    PID_calc(&triggerMotorPID, presentVTriggerMotor, wantedVTriggerMotor);
    giveTriggerCurrent = triggerMotorPID.out;
}

#ifdef STM_STUDIO_SHOOT_SPEED
static int16_t fricSpeedL;
static int16_t fricSpeedR;
#endif

static void get_shoot_motor_speed(void)
{
#ifdef SENTRY
    presentVShootMotor[0] = get_motor_measure_point(SHOOT_DOWN_FRIC_L)->speed_rpm;
    presentVShootMotor[1] = get_motor_measure_point(SHOOT_DOWN_FRIC_R)->speed_rpm;
    presentVTriggerMotor = get_motor_measure_point(SHOOT_DOWN_TRIGGER)->speed_rpm;
#else
    presentVShootMotor[0] = get_motor_measure_point(SHOOT_FRIC_L)->speed_rpm;
    presentVShootMotor[1] = get_motor_measure_point(SHOOT_FRIC_R)->speed_rpm;
    presentVTriggerMotor = get_motor_measure_point(SHOOT_TRIGGER)->speed_rpm;
#endif
    first_order_filter_cali(fricFilter, presentVShootMotor[0]);
    first_order_filter_cali(fricFilter + 1, presentVShootMotor[1]);
    presentVShootMotor[0] = fricFilter[0].out;
    presentVShootMotor[1] = fricFilter[1].out;
#ifdef STM_STUDIO_SHOOT_SPEED
    fricSpeedL = presentVShootMotor[0];
    fricSpeedR = presentVShootMotor[1];
#endif
}

void shoot_task(void const *pvParameters)
{
    osDelay(SHOOT_TASK_INIT_TIME);
    init_shoot_PIDs();  // 初始化PID
    init_shoot_modes(); // 初始化发射状态
    init_shoot_filter();
    robotMode = get_robot_present_mode();
    rc_p = get_remote_control_point(); // 获取遥控器数据和NUC数据指针
    nuc_p = get_nuc_control_point();
    remoteControl_p = get_usart_remote_control_p();
#ifdef GIMBAL_WITHOUT_IMU
    fricUpCurrent_p = get_fric_up_current_p();
    triggerUpCurrent_p = get_trigger_up_current_p();
#endif
    while (1)
    {
        get_shoot_motor_speed();
        get_last_has_been_shoot();
        get_instruction_and_buff(); // 获取指令并缓冲
        check_NUC_offLine_and_set_insBuf();
        detect_fric_offLine();
#ifdef WITH_REFEREE
        heat_detect();
#endif
        fric_mode_change();    // 摩擦轮状态改变
        trigger_mode_change(); // 拨弹轮状态转变
        set_speed_by_mode();
        calc_give_current(); // 计算PID
#ifdef ZERO_CURRENT_SAFE_SHOOT
        zeroCurrentMark = 1;
#endif
#ifndef SENTRY
        if (zeroCurrentMark)
        {
            CAN_cmd_shoot(0, 0);
            giveTriggerCurrent = 0;
        }
        else
        {
            CAN_cmd_shoot(giveShootCurrent[0], giveShootCurrent[1]);
        }
#else
        if (zeroCurrentMark)
        {
            CAN_cmd_shoot_up_stm(0);
            CAN_cmd_GIMBAL_CAN_M_up(0, 0, 0);
            CAN_cmd_GIMBAL_CAN_M_down(0, 0);
        }
        else
        {
#ifdef GIMBAL_WITHOUT_IMU
            CAN_cmd_shoot_up_stm(*triggerUpCurrent_p);
            CAN_cmd_GIMBAL_CAN_M_up(fricUpCurrent_p[0], fricUpCurrent_p[1], giveTriggerCurrent);
            CAN_cmd_GIMBAL_CAN_M_down(giveShootCurrent[0], giveShootCurrent[1]);
#else
            CAN_cmd_shoot_up_stm(0);
            CAN_cmd_GIMBAL_CAN_M_up(0, 0, giveTriggerCurrent);
            CAN_cmd_GIMBAL_CAN_M_down(giveShootCurrent[0], giveShootCurrent[1]);
#endif
        }
#endif
        osDelay(SHOOT_CTRL_TIME);
    }
}

#ifndef SENTRY
const int16_t *get_trigger_current_p(void)
{
    return &giveTriggerCurrent;
}
#endif

static void monitor_trigger_rounds(void)
{
    triggerCtrl.nowECD = *(triggerCtrl.ECDPoint);
    triggerCtrl.stepEcd = triggerCtrl.nowECD - triggerCtrl.lastEcd;
    while (triggerCtrl.stepEcd < -ECD_FULL_ROUND / 2)
    { // 按设定转速不可能超过
        triggerCtrl.stepEcd += ECD_FULL_ROUND;
    }
    while (triggerCtrl.stepEcd > ECD_FULL_ROUND / 2)
    {
        triggerCtrl.stepEcd -= ECD_FULL_ROUND;
    }
    triggerCtrl.nowRounds += (fp32)triggerCtrl.stepEcd / ECD_FULL_ROUND;
    if (triggerCtrl.nowRounds > TRIGGER_ROUNDS * 1000)
    {
        triggerCtrl.nowRounds -= TRIGGER_ROUNDS * 1000;
    }
    if (triggerCtrl.nowRounds < -TRIGGER_ROUNDS * 1000)
    {
        triggerCtrl.nowRounds += TRIGGER_ROUNDS * 1000;
    }
    triggerCtrl.lastEcd = triggerCtrl.nowECD;
}

void shoot_trigger_monitor(void const *pvParameters)
{
    osDelay(SHOOT_TASK_INIT_TIME);
    init_trigger_ECD_rounds_monitor(); // 初始化拨弹轮圈数监控
    while (1)
    {
        monitor_trigger_rounds();
        osDelay(1);
    }
}

#endif
